//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================
#include <ddk.h>
#include <bulverde.h>
#include "freq.h"

#define udelay(u)   DzDelay(u)
#define mdelay(m)   DzDelay(m * 1000)

#define _HAL_EnterCritical(s)   (s = 1)
#define _HAL_LeaveCritical(s)   (s = 0)

EXTERN_C int ReadCLKCFG(void);
EXTERN_C void WriteCLKCFG(int);
EXTERN_C void WritePWRMODE(int);

#define	volt_GPIO116		(1<<20)
#define 	volt_GPIO120		(1<<24)
#define CPLOCK (1<<29)
#define PPLOCK (1<<28)
#define ICMR_FFUART (1<<22)
#define ICMR_GPIOX (1<<10)
#define ICMR_KEYPAD (1<<4)
#define ICMR_RTC_AL (1<<31)

#define ICMR_OSTIMER0   (1<<26)



EXTERN_C unsigned long current_freq;
unsigned long icmr;
unsigned long idle_t1=0,idle_t2=0;

int Voltage_GPIO(void)
{
	GPSR3 |= volt_GPIO116|volt_GPIO120;
	GPDR3 |= volt_GPIO116|volt_GPIO120;
	GAFR3_U &=~((3<<8)|(3<<16));
	return 0;

}
void ChangeVoltage(unsigned long mode)
{
	switch (mode)
	{
		case VOLT_MIN:
			GPCR3 |=volt_GPIO116|volt_GPIO120;
			break;
		case VOLT_LOW:
			GPSR3 |= volt_GPIO116;
			GPCR3 |= volt_GPIO120;
			break;
		case VOLT_HIGH:
			GPSR3 |=volt_GPIO120;
			GPCR3 |= volt_GPIO116;
			break;
		case VOLT_MAX:
			GPSR3 |=volt_GPIO116|volt_GPIO120;
			break;
		default:
			break;
	}
	(void) GPLR3;

	udelay(1000); //wait for the voltage to be stable
}

void enter_13M_mode(void)
{
	unsigned long value;
	int s;
	value = CCCR;
	value |= (CCCR_CPDIS|CCCR_LCD26);
	value &=~CCCR_PPDIS;
	CCCR = value;
	(void*)CCCR;
	_HAL_EnterCritical(s);

    //asm("mov	r0, #0x2");   	     		//set to Frequency Change Mode
	//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
	WriteCLKCFG(0x2);
	_HAL_LeaveCritical(s);
	mdelay(1);//wait for it to be stable

}

void exit_13M_mode(void)
{
	int s = 0;

	CCCR |=(CCCR_CPDIS|CCCR_PLL_EARLY_EN);
	(void*) CCCR;

	while((CCSR&(CPLOCK|PPLOCK))==0){}

	CCCR &=~(CCCR_CPDIS|CCCR_PPDIS);
	(void *) CCCR;

	_HAL_EnterCritical(s);
	//asm("mov	r0, #0x2");   	     		//set to Frequency Change Mode
	//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
	WriteCLKCFG(0x2);
	_HAL_LeaveCritical(s);
	mdelay(1);//wait for it to be stable
}

//#define TOSHIBA_PIX_CLK 	650
//void change_lcd_clk(struct core_freq freq)
//{
//	unsigned long CCCR_L, LCLK, PCD, lccr3;
//	CCCR_L = freq.L&0x1F;
//
//
//
//	if (CCCR_L < 8) // L = [2 - 7]
//		LCLK = (13 * CCCR_L) * 100;
//	else if (CCCR_L < 17) // L = [8 - 16]
//		LCLK = ((13 * CCCR_L) * 100) >> 1;
//	else if (CCCR_L < 32) // L = [17 - 31]
//		LCLK = ((13 * CCCR_L) * 100) >> 2;
//
//	PCD = (LCLK / (2 * TOSHIBA_PIX_CLK));
//	lccr3 = LCCR3;
//	lccr3 &= ~ 0xff;
//	lccr3 |= PCD;
//	LCCR3 = lccr3;
//	udelay(1000);
//}

void ChangeFreq(struct core_freq freq){
		int value;
		unsigned long mdrefr_val, /*cccr_val = 0, clkcfg_val = 0,*/ msc0_val, mdrefr_dri = 0;
		unsigned long L, /*M = 0,*/ N2, memclk;

		value = CCCR;
		value &= 0x3FFFFFFF; 				//make sure the Core and Peripheral PLL disable bits are clear.
		value = value & 0xFFFFF000;			//clear the L and 2N bits

		L = freq.L & 0x1F;						//make sure L is only 0 thru 31
		value |= L; 						//OR in the L value

		N2 = freq.twoN & 0xf;					//make sure 2N is only 0 thru 15

		value |= (N2 << 7);				//OR in the 2N value
		if(freq.frequency == 91)
			value &= ~CCCR_A;
		else
			value |=CCCR_A;


		/* default setting */
		msc0_val 	= MSC0;
		mdrefr_val	= MDREFR;


		/* MEM clock frequency = System Bus Frequence */
		if (freq.b)
			memclk = (L*13);
		else
			memclk = (L*13)/2;

		if(freq.frequency == 91)
			memclk = 91;


		if (memclk <= 52) {
		/* SDCLK0 = MEMCLK */
			mdrefr_val &= ~(MDREFR_K0DB2 | MDREFR_K0DB4);
		}
		else if (memclk == 91)
		{
			mdrefr_val |= MDREFR_K0DB4;
			mdrefr_val |= MDREFR_K0DB2;
			mdrefr_val |= MDREFR_K1DB2;
		}
		else if (memclk == 104) {
			mdrefr_val &= ~MDREFR_K0DB4;
			mdrefr_val |= MDREFR_K0DB2;
			mdrefr_dri = mdrefr_val;
			mdrefr_val |= 0xfff;
		}else{
			/* SDCLK0 = MEMCLK/4 */
			mdrefr_val |= MDREFR_K0DB4;

			/* SDCLK1 = MEMCLK/2 - forced */
			mdrefr_val |= MDREFR_K1DB2;
		}

		/* adjust MSC0 for bank 0 */

		msc0_val &= ~(0x7f00);
		msc0_val |= 0x5a00;


		MSC0 = msc0_val;
		(void) MSC0;

		MDREFR = mdrefr_val;
		(void) MDREFR;
		MDREFR = (MDREFR & 0x3FDFEFFF);
		(void) MDREFR;

		CCCR = value;					//now set the CCCR register
		(void *)CCCR;
		if ((freq.turbo==0) && (freq.b==0)){
    		//asm("mov	r0, #0x2");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0x2);
		}

		if((freq.turbo==0)&&(freq.b!=0)){
		    //asm("mov	r0, #0xA");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0xA);
		}

		if ((freq.turbo!=0) && (freq.b==0)){
    		//asm("mov	r0, #0x3");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0x3);
		}

		if ((freq.turbo!=0) && (freq.b!=0)){
    		//asm("mov	r0, #0xB");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0xB);
		}
		udelay(1000); //wait for the voltage to be stable
		if(memclk == 104)
		{
			mdrefr_val = mdrefr_dri;
			mdrefr_val &= ~MDREFR_K1DB2;
			MDREFR = mdrefr_val;
			(void) MDREFR;
			MDREFR = (MDREFR & 0x3FDFEFFF);
			(void) MDREFR;
		}
		else if(freq.frequency != 91)
		{
			MDREFR = mdrefr_val;
			(void) MDREFR;
			MDREFR = (MDREFR & 0x3FDFEFFF);
			(void) MDREFR;
		}

		udelay(1000); //wait for the voltage to be stable
}


void Change_Freq_Volt(struct core_freq freq)
{
    kprintf("Change_Freq_Volt %d --> %d\n", current_freq, freq.frequency);

	int s;
	if(current_freq == freq.frequency)
		return;

	if(current_freq > freq.frequency){
		_HAL_EnterCritical(s);
		//change_lcd_clk(freq);
		ChangeFreq(freq);
		ChangeVoltage(freq.volt);
		current_freq = freq.frequency;
		_HAL_LeaveCritical(s);
		return;
	}
	else{
		_HAL_EnterCritical(s);
		ChangeVoltage(freq.volt);
		ChangeFreq(freq);
		//change_lcd_clk(freq);
		current_freq = freq.frequency;
		_HAL_LeaveCritical(s);
		return;
	}

}
void pm_idle_summy(unsigned long idle_t);
EXTERN_C void enter_idle_mode(void)
{

//	ICCR = 1;
//	icmr = ICMR;//
//	ICMR = (ICMR_FFUART|ICMR_GPIOX|ICMR_KEYPAD|ICMR_RTC_AL|ICMR_OSTIMER0);
//	(void *) ICMR;

	//kprintf("idle\n");

	//asm("mov	r0, #0x1");   	     		//set to idle mode
    //asm("mcr	p14, 0, r0, c7, c0, 0");	//write to the PWRMODE coprocessor reg
    //idle_t1 = OSCR;
    WritePWRMODE(0x1);
    //unsigned long t2 = OSCR;

//	ICMR = icmr;
    //pm_idle_summy((3*( idle_t2-idle_t1))>>10);
	//kprintf("Exit from idle mode, idle %d ms\n",(3*( idle_t2-idle_t1))>>10);
    return ;
}
